// Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

/*
 * Author: Luca Marchionni
 * Author: Bence Magyar
 * Author: Enrique Fernández
 * Author: Paul Mathieu
 */

#ifndef FOUR_WHEEL_STEERING_CONTROLLER__ODOMETRY_HPP_
#define FOUR_WHEEL_STEERING_CONTROLLER__ODOMETRY_HPP_

#include <cmath>

#include "rclcpp/time.hpp"
#include "rcppmath/rolling_mean_accumulator.hpp"

namespace four_wheel_steering_controller {
    class Odometry {
    public:
        explicit Odometry(size_t velocity_rolling_window_size = 10);

        void init(const rclcpp::Time& time);
        bool update(double(&wheel_pos)[4], double(&steering_vel)[4], const rclcpp::Time& time);
        bool updateFromVelocity(double(&wheel_vel)[4], double(&steering_vel)[4], const rclcpp::Time& time);
        void updateOpenLoop(double linear, double angular, const rclcpp::Time& time);
        void resetOdometry();

        double getX() const { return x_; }
        double getY() const { return y_; }
        double getHeading() const { return heading_; }
        double getLinearX() const { return linear_x_; }
        double getLinearY() const { return linear_y_; }
        double getAngular() const { return angular_; }

        void setWheelParams(
            double         wheel_base_,
            double         wheel_track_,
            double         wheel_radius_,
            int wheel_num = 4);
        void setVelocityRollingWindowSize(size_t velocity_rolling_window_size);

    private:
        using RollingMeanAccumulator = rcppmath::RollingMeanAccumulator<double>;

        void integrateRungeKutta2(double linear, double angular);
        void integrateRungeKutta2(double linear_x, double linear_y, double angular);
        void integrateExact(double linear, double angular);
        void integrateExact(double linear_x, double linear_y, double angular);
        void resetAccumulators();

        // Current timestamp:
        rclcpp::Time timestamp_;

        // Current pose:
        double x_;        //   [m]
        double y_;        //   [m]
        double heading_;  // [rad]

        // Current velocity:
        double linear_;   //   [m/s]
        double linear_x_;   //   [m/s]
        double linear_y_;   //   [m/s]
        double angular_;  // [rad/s]

        // Wheel kinematic parameters [m]:
        double  wheel_track_;
        double  wheel_base_;
        double  wheel_radius_;
        int         wheel_num_;



        // Previous wheel position/state [rad]:
        double wheel_old_pos_[4];
        double steering_old_pos_[4];

        // Rolling mean accumulators for the linear and angular velocities:
        size_t velocity_rolling_window_size_;
        RollingMeanAccumulator linear_x_accumulator_;
        RollingMeanAccumulator linear_y_accumulator_;
        RollingMeanAccumulator angular_accumulator_;
    };

}  // namespace four_wheel_steering_controller

#endif  // FOUR_WHEEL_STEERING_CONTROLLER__ODOMETRY_HPP_
